/*
 * velocityDrive.cpp
 *
 *  Created on: Sep 27, 2012
 *      Author: bouchier
 *
 *  This program subscribes to topics that publish a Velocity message, which is defined as:
 *  float32 linear
 *  float32 angular
 *  and drives the jrover robot at the requested velocities.
 *  Note: the Velocity message is used by turtlebot, so many turtlebot apps should
 *  be able to drive jrover.
 *
 *  This program subscribes to the test_motor topic, which can be used to drive individual motors
 *  for test purposes. Publish the desired value with rostopic as follows. The first value is
 *  motor number, the second is the value. Use the -- syntax for negative values.
 *  rostopic pub test_motor -1 jrover/Velocity 1 100
 *  rostopic pub test_motor -1 jrover/Velocity -- 2 -100
 */

#include <ros.h>
#include <Velocity.h>
#include <iostream>
#include <cmath>
#include "Servo.h"
using namespace std;

/*
 * Wiring assumptions:
 * Motor ports
 * 15: left front drive
 * 3: left front steer
 * 16: right front drive
 * 1: right front steer
 * 2: left middle drive
 * 7: left middle steer
 * 12: right middle drive
 * 6: right middle steer
 * 14: left back drive
 * 4: left back steer
 * 13: right back drive
 * 5: right back steer
 */

// Motor indexes
#define LF_DRIVE 15
#define LF_STEER 3
#define RF_DRIVE 16
#define RF_STEER 1
#define LM_DRIVE 2
#define LM_STEER 7
#define RM_DRIVE 12
#define RM_STEER 6
#define LB_DRIVE 14
#define LB_STEER 4
#define RB_DRIVE 13
#define RB_STEER 5

/*
 * Motor direction:
 * Increasing servo value (i.e. 0 -> 255) makes servo move clockwise from shaft, i.e. CCW from back.
 * This corresponds +ve change in radians, so does not need to be reversed. For clarity, the
 * rear steering is not reversed either, but instead is commanded to turn in the opposite
 * direction of the front steering.
 *
 * Increasing motor value makes a motor turn counterclockwise looking at shaft. This means
 * left motors will drive forward with +ve values of velocity, ane the right motors need
 * to be reversed.
 */
ServoMotor lfDrive = ServoMotor(LF_DRIVE, 128, 6.35, 127, false);
ServoMotor lfSteer = ServoMotor(LF_STEER, 128, 2.54, 127, false);
ServoMotor rfDrive = ServoMotor(RF_DRIVE, 128, 6.35, 127, true);
ServoMotor rfSteer = ServoMotor(RF_STEER, 128, 2.54, 127, false);
ServoMotor lmDrive = ServoMotor(LM_DRIVE, 128, 6.35, 127, false);
ServoMotor lmSteer = ServoMotor(LM_STEER, 128, 2.54, 127, false);
ServoMotor rmDrive = ServoMotor(RM_DRIVE, 128, 6.35, 127, true);
ServoMotor rmSteer = ServoMotor(RM_STEER, 128, 2.54, 127, false);
ServoMotor lbDrive = ServoMotor(LB_DRIVE, 128, 6.35, 127, false);
ServoMotor lbSteer = ServoMotor(LB_STEER, 128, 2.54, 127, false);
ServoMotor rbDrive = ServoMotor(RB_DRIVE, 128, 6.35, 127, true);
ServoMotor rbSteer = ServoMotor(RB_STEER, 128, 2.54, 127, false);

ros::NodeHandle  nh;
char *rosSrvrIp = "192.168.11.9";

/*
 * driveRover takes a requested robot linear and angular velocity and asks each motor to drive
 * to the appropriate position/speed to move the robot as requested. It uses the following
 * steering and wheel speed equations:
 *
 * ---
 * R = linear/angular
 *
 * R: robot turn radius in inches
 * linear: robot linear speed at center of middle wheels in in/s;
 * angular: robot angular velocity in rad/s
 * ---
 *
 * vRM = linear + 0.5 * (angular * WIDTH)
 * vLM = linear - 0.5 * (angular * WIDTH)
 *
 * vRM, vLM: speed of right-middle, left-middle motors in in/s
 * WIDTH: distance between adjacent wheels
 * ---
 *
 * thetaLF = atan(LENGTH_FRONT/(R - (0.5 * WIDTH))) * 180 / PI
 * thetaRF = atan(LENGTH_FRONT/(R + (0.5 * WIDTH))) * 180 / PI
 * thetaLB = atan(LENGTH_BACK/(R - (0.5 * WIDTH))) * 180 / PI
 * thetaRB = atan(LENGTH_BACK/(R + (0.5 * WIDTH))) * 180 / PI
 *
 * thetaLF, thetaRF, thetaLB, thetaRB: angle of left-front, right-front,
 *   left-back, right-back steering wheels in degrees
 * LENGTH_FRONT: distance between middle and front axles
 * LENGTH_BACK: distance between middle and back axles
 * Note inaccuracy; if LENGTH_FRONT/BACK changes as wheels steer the calculation will be incorrect
 * ---
 *
 * vLF = angular * (LENGTH_FRONT/sin(thetaLF))
 * vRF = angular * (LENGTH_FRONT/sin(thetaRF))
 * vLB = angular * (LENGTH_BACK/sin(thetaLB))
 * vRB = angular * (LENGTH_BACK/sin(thetaRB))
 *
 * vLF, vRF, vLB, vRB: speed of left-fron, right-front, left-back, right-back
 *   wheels in in/s
 *
 * =====
 * Test stimuli & expected results
 *
 * $ rostopic pub driveRover -1 jrover/Velocity -- 10 0.1
 * Requested linear speed: 10.00, requested angular speed: 0.10, turn radius: 100.0
 * LF: (9.80, 5.9)         RF: (10.30, 5.6)
 * LM: 9.75                RF: 10.25
 * LB: (9.95, -11.6)       RB: (10.44, -11.0)
 *
 * $ rostopic pub driveRover -1 jrover/Velocity -- 10 -0.1
 * Requested linear speed: 10.00, requested angular speed: -0.10, turn radius: -100.0
 * LF: (10.30, -5.6)       RF: (9.80, -5.9)
 * LM: 10.25               RF: 9.75
 * LB: (10.44, 11.0)       RB: (9.95, 11.6)
 *
 * $ rostopic pub driveRover -1 jrover/Velocity -- -10 0.1
 * Requested linear speed: -10.00, requested angular speed: 0.10, turn radius: -100.0
 * LF: (-10.30, -5.6)      RF: (-9.80, -5.9)
 * LM: -10.25              RF: -9.75
 * LB: (-10.44, 11.0)      RB: (-9.95, 11.6)
 *
 * $ rostopic pub driveRover -1 jrover/Velocity -- -10 -0.1
 * Requested linear speed: -10.00, requested angular speed: -0.10, turn radius: 100.0
 * LF: (-9.80, 5.9)                RF: (-10.30, 5.6)
 * LM: -9.75               RF: -10.25
 * LB: (-9.95, -11.6)              RB: (-10.44, -11.0)
 *
 * @param linear The requested linear speed in in/s
 * @param angular The requested angular velocity in rad/s
 * @return 0 for success, -1 if the request resulted in trying to drive any motor beyond it's limit
 */
#define WIDTH 5
#define LENGTH_FRONT 10
#define LENGTH_BACK 20
#define PI 3.14159265

int driveRover(float linear, float angular)
{
	float R;		// robot turn radius in inches
	float vLM, vRM; //speed of right-middle, left-middle motors in in/s
	float thetaLF, thetaRF, thetaLB, thetaRB;	// angle of left-front, right-front, left-back, right-back steering wheels in degrees
	float thetaLFr, thetaRFr, thetaLBr, thetaRBr;	// angle of left-front, right-front, left-back, right-back steering wheels in radians
	float vLF, vRF, vLB, vRB; // speed of left-front, right-front, left-back, right-back wheels in in/s
	int rv;

	// calculate robot turn radius
	R = linear/angular;
	printf("Requested linear speed: %0.2f, requested angular speed: %0.2f, turn radius: %0.1f\n", linear, angular, R);

	// calculate middle drive motor speeds
	vRM = linear + 0.5 * (angular * WIDTH);
	vLM = linear - 0.5 * (angular * WIDTH);

	// calculate steered wheel angles
	thetaLFr = atan(LENGTH_FRONT/(R - (0.5 * WIDTH)));
	thetaRFr = atan(LENGTH_FRONT/(R + (0.5 * WIDTH)));
	thetaLBr = 0 - (atan(LENGTH_BACK/(R - (0.5 * WIDTH))));
	thetaRBr = 0 - (atan(LENGTH_BACK/(R + (0.5 * WIDTH))));
	thetaLF = thetaLFr * 180 / PI;
	thetaRF = thetaRFr * 180 / PI;
	thetaLB = thetaLBr * 180 / PI;
	thetaRB = thetaRBr * 180 / PI;

	// calculate steered wheel speeds
	vLF = angular * (LENGTH_FRONT/sin(thetaLFr));
	vRF = angular * (LENGTH_FRONT/sin(thetaRFr));
	vLB = angular * (LENGTH_BACK/sin(0 - thetaLBr));
	vRB = angular * (LENGTH_BACK/sin(0 - thetaRBr));

	printf("LF: (%0.2f, %0.1f)\t\tRF: (%0.2f, %0.1f)\n", vLF, thetaLF, vRF, thetaRF);
	printf("LM: %0.2f\t\tRF: %0.2f\n", vLM, vRM);
	printf("LB: (%0.2f, %0.1f)\t\tRB: (%0.2f, %0.1f)\n", vLB, thetaLB, vRB, thetaRB);

	// command the motors to run
	rv = 0;
	rv += lfDrive.set(vLF);
	rv += lfSteer.set(thetaLF);
	rv += rfDrive.set(vRF);
	rv += rfSteer.set(thetaRF);
	rv += lmDrive.set(vLM);
	rv += lmSteer.set(0);
	rv += rmDrive.set(vRM);
	rv += rmSteer.set(0);
	rv += lbDrive.set(vLB);
	rv += lbSteer.set(thetaLB);
	rv += rbDrive.set(vRB);
	rv += rbSteer.set(thetaRB);
	if (rv != 0)
		printf("driveRover->set returned %d", rv);

	return(0);
}

/*
 * driveMotorMsgCb is called when the Velocity message is published to the driveRover topic.
 * It calls driveRover to calculate and apply the angle and speed for each motor, with the
 * result that the robot drives at the requested linear and angular velocities.
 */
void driveRoverMsgCb(const jrover::Velocity& velocity_msg){
	float linear = velocity_msg.linear;
	float angular = velocity_msg.angular;
	driveRover(linear, angular);
}
ros::Subscriber<jrover::Velocity> driveRoverSub("driveRover", driveRoverMsgCb );

/*
 * motorTestMsgCb is called when the Velocity message is published to the motor_test topic. It treats the linear variable
 * as a motor ID, and the angular variable as a value, and applies the value to the target motor.
 * The following commands swing servo from -40 - +40 degrees then to 0 (centered):
 * $ rostopic pub test_motor -1 jrover/Velocity -- 2 -40
 * $ rostopic pub test_motor -1 jrover/Velocity -- 2 40
 * $ rostopic pub test_motor -1 jrover/Velocity -- 2 0
 */
void motorTestMsgCb(const jrover::Velocity& velocity_msg){
	ServoMotor *targetMotor;
	int rv;

	float linear = velocity_msg.linear;
	float angular = velocity_msg.angular;
	int servoNum = (int)(linear + 0.5);
	printf("Received subscribed test_motor, motor %d, value %f\n", servoNum, angular);
	servoNum--;		// convert from motor number to motor index
	switch (servoNum) {
	case LF_DRIVE: targetMotor = &lfDrive; break;
	case LF_STEER: targetMotor = &lfSteer; break;
	case RF_DRIVE: targetMotor = &rfDrive; break;
	case RF_STEER: targetMotor = &rfSteer; break;
	case LM_DRIVE: targetMotor = &lmDrive; break;
	case LM_STEER: targetMotor = &lmSteer; break;
	case RM_DRIVE: targetMotor = &rmDrive; break;
	case RM_STEER: targetMotor = &rmSteer; break;
	case LB_DRIVE: targetMotor = &lbDrive; break;
	case LB_STEER: targetMotor = &lbSteer; break;
	case RB_DRIVE: targetMotor = &rbDrive; break;
	case RB_STEER: targetMotor = &rbSteer; break;
	default: printf("Invalid motor ID %d\n", servoNum); return;
	}
	rv = targetMotor->set(angular);
	if (rv != 0)
		printf("targetMotor->set returned %d", rv);
}
ros::Subscriber<jrover::Velocity> motorTestSub("test_motor", motorTestMsgCb );

void servoTest()
{
	int error = 0;
	error += lfDrive.set(1.0);
	sleep(2);
	error += lfDrive.set(-1.0);
	sleep(2);
	error += lfDrive.set(0.5);
	sleep(2);
	error += lfDrive.set(-0.5);
	sleep(2);
	error += lfDrive.set(0.0);
	sleep(2);
	printf("servoTest ended with error value: %d\n", error);
}

int main() {
	servoTest();
	nh.initNode(rosSrvrIp);
	nh.subscribe(driveRoverSub);
	nh.subscribe(motorTestSub);

	while(1) {
		  sleep(1);
		  nh.spinOnce();
	}
}

